// Copyright (c) 2012, http://code.google.com/p/example-xplane-plugins/
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of "example-xplane-plugins" nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL "example-xplane-plugins" BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


/*------------------------------------------------------------------------*/
// INSTRUCTIONS - GENERAL USAGE
/*------------------------------------------------------------------------*/

// USAGE: (XPLANE)
// ENABLE plugin to CREATE the shared memory interface.
// DISABLE plugin to DESTROY the shared memory interface.

// USAGE: (MATLAB)
// CONNECT to an EXISTING shared memory interface using XPIMInit()
// CAPTURE an image using XPIMCaptureImage()
// DISCONNECT from the shared memory interface using XPIMCleanup()

/*------------------------------------------------------------------------*/
// INSTRUCTIONS - COMPILATION OF THIS FILE
/*------------------------------------------------------------------------*/

// COMPILATION: (OCTAVE)
// Switch to the "src" folder where XPScreenGrabMEX.cpp and XPScreenGrabConstants.h exists
//
//   mkoctfile --mex XPScreenGrabMEX.cpp -IC:\boost\include\boost-1_48\ -LC:\boost\lib\
//
// NOTE: Adjust the boost paths depending on where it was "installed" (in this case C:\boost\)


/*------------------------------------------------------------------------*/
// INCLUDES
/*------------------------------------------------------------------------*/

// Matlab or Octave
#include "mex.h"

// Boost - for shared memory comms
#include <boost/interprocess/shared_memory_object.hpp>
#include <boost/interprocess/sync/named_mutex.hpp>

// Plugin - shared constants 
#include "XPScreenGrabConstants.h"

/*------------------------------------------------------------------------*/
// NAMESPACES
/*------------------------------------------------------------------------*/

using namespace boost::interprocess;

/*------------------------------------------------------------------------*/
// STATIC CONSTANTS
/*------------------------------------------------------------------------*/

enum eReadyness
{
  eERROR          = -1,
  eUNINITIALISED  =  0,
  eREADY          = +1
};

static const int XP_INIT(0);
static const int XP_GRAB(1);
static const int XP_CLEANUP(2);

static const int NUM_INPUT_PARAMS(1);
static const int NUM_OUTPUT_PARAMS(1);
static const int COMMAND_PARAM_INDEX(0);
static const int IMAGE_PARAM_INDEX(0);

static const int NUM_IMAGE_DIMENSIONS(3);

/*------------------------------------------------------------------------*/
// STATIC VARIABLES
/*------------------------------------------------------------------------*/

static mapped_region* s_region = NULL;
static named_mutex*   s_mutex = NULL;

static eReadyness s_readyness(eUNINITIALISED);
static mxArray*   s_ptrMatrixPri(NULL);
static mxArray*   s_ptrMatrixSec(NULL);

/*------------------------------------------------------------------------*/
// FILE SCOPE FUNCTION DECLARATIONS
/*------------------------------------------------------------------------*/

static bool XPInit();
static bool XPCleanup();
static bool XPGrab(int nlhs, mxArray *plhs[]);

/*------------------------------------------------------------------------*/
// IMPLEMENTATION
/*------------------------------------------------------------------------*/

// Allows a quick compilation check in VS2005 (only linker errors should remain)
#ifdef USE_MAIN
int main()
{
}
#endif

// Entry point for Matlab MEX function (the MEX compiler replaces it with filename)
void mexFunction (int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[])
{
  bool success = true;

  if (success)
  {
    if (NUM_INPUT_PARAMS!=nrhs)
    {
      mexPrintf("Incorrect number of inputs (Got %i Expected %i).\n", nrhs, NUM_INPUT_PARAMS);
      success = false;
    }
  }

  if (success)
  {
    if (NULL==prhs)
    {
      mexPrintf("Invalid MEX pointer 'prhs'.\n");
      success = false;
    }
  }

  if (success)
  {
    if (NULL==prhs[COMMAND_PARAM_INDEX])
    {
      mexPrintf("First input param is null pointer.\n");
      success = false;
    }
  }

  if (success)
  {
    int command(static_cast<int>(mxGetScalar(prhs[COMMAND_PARAM_INDEX])));

    switch (command)
    {
    case XP_INIT:
      {
        // Can call any time
        success = XPInit();
        if (success)
        {
          s_readyness = eREADY;
        }
        else
        {
          mexPrintf("Init failed.\n");
          s_readyness = eERROR;
        }
      }
      break;
    case XP_GRAB:
      {
        if (eREADY==s_readyness)
        {
          success = XPGrab(nlhs, plhs);
        }
        else
        {
          mexPrintf("Call init before grabbing.\n");
          s_readyness = eERROR;
        }
      }
      break;
    case XP_CLEANUP:
      {
        // Can call any time
        success = XPCleanup();
        if (success)
        {
          s_readyness = eUNINITIALISED;
        }
        else
        {
          mexPrintf("Cleanup failed.\n");
          s_readyness = eERROR;
        }
      }
      break;   
    default:
      {
        mexPrintf("Unknown command.\n");
        success = false;
        s_readyness = eERROR;
      }
    }
  }

  if (!success)
  {
    mexPrintf("Some errors occurred.\n");
  }
}

// CONNECTS TO SHARED MEMORY INTERFACE
static bool XPInit()
{
  bool success = XPCleanup();

  if (success)
  {
    // When specified in this order this map to an RGBRGBRGB pattern which is the glReadPixel output
    mwSize dimsPri[NUM_IMAGE_DIMENSIONS] = 
    {
      SCREENSHOT_NUM_COLOR_PLANES,
      SCREENSHOT_WIDTH,
      SCREENSHOT_HEIGHT
    };

    s_ptrMatrixPri = mxCreateNumericArray(NUM_IMAGE_DIMENSIONS, dimsPri, mxUINT8_CLASS, mxREAL);  
    if (NULL!=s_ptrMatrixPri)
    {
      mexMakeArrayPersistent(s_ptrMatrixPri);
    }
    else
    {
      mexPrintf("Failed to created an array to receive OpenGL frame.\n");
      success = false;
    }
  }

  if (success)
  {
    // When specified in this order this map to an RGBRGBRGB pattern which is the glReadPixel output
    mwSize dimsSec[NUM_IMAGE_DIMENSIONS] = 
    {
      SCREENSHOT_HEIGHT,
      SCREENSHOT_WIDTH,
      SCREENSHOT_NUM_COLOR_PLANES
    };

    s_ptrMatrixSec = mxCreateNumericArray(NUM_IMAGE_DIMENSIONS, dimsSec, mxUINT8_CLASS, mxREAL);  
    if (NULL!=s_ptrMatrixSec)
    {
      mexMakeArrayPersistent(s_ptrMatrixSec);
    }
    else
    {
      mexPrintf("Failed to created an array to return.\n");
      success = false;
    }
  }

  if (success)
  {
    try
    {
      // Try to get the named mutex
      s_mutex = new named_mutex(open_only, SHARED_MUTEX_NAME);
    }
    catch (interprocess_exception& err)
    {
      mexPrintf("Failed to find named_mutex ('%s').\n", SHARED_MUTEX_NAME);
      mexPrintf("REASON: %s\n", err.what());
      mexPrintf("Ensure the XPlane plugin is running.\n");
      success = false;
    }
    catch (...)
    {
      mexPrintf("Failed to find named_mutex ('%s').\n", SHARED_MUTEX_NAME);
      mexPrintf("REASON: %s\n", "UNKNOWN EXCEPTION!");
      mexPrintf("Ensure the XPlane plugin is running.\n");
      success = false;
    }
  }

  if (success)
  {
    try
    {
      // Try to get the shared memory segment
      shared_memory_object shm_obj(
        open_only,                 //only create
        SHARED_MEMORY_NAME,          //name
        read_write);                 //read-write mode

      //Map the whole shared memory in this process
      s_region = new mapped_region(shm_obj, read_only);
    }
    catch (interprocess_exception& err)
    {
      mexPrintf("Failed to find shared_memory_object ('%s').\n", SHARED_MEMORY_NAME);
      mexPrintf("REASON: %s\n", err.what());
      mexPrintf("Ensure the XPlane plugin is running.\n");
      success = false;
    }
    catch (...)
    {
      mexPrintf("Failed to find shared_memory_object ('%s').\n", SHARED_MEMORY_NAME);
      mexPrintf("REASON: %s\n", "UNKNOWN EXCEPTION!");
      mexPrintf("Ensure the XPlane plugin is running.\n");
      success = false;
    }
  }

  return success;
}

// DISCONNECTS FROM SHARED MEMORY INTERFACE
static bool XPCleanup()
{
  bool success = true;

  if (NULL!=s_ptrMatrixPri)
  {
    mxDestroyArray(s_ptrMatrixPri);
    s_ptrMatrixPri = NULL;
  }

  if (NULL!=s_ptrMatrixSec)
  {
    mxDestroyArray(s_ptrMatrixSec);
    s_ptrMatrixSec = NULL;
  }

  if (NULL!=s_region)
  {
    delete s_region;
    s_region = NULL;
  }

  return success;
}

// READ THE SHARED MEMORY INTERFACE INTO A MATLAB IMAGE ARRAY
static bool XPGrab(int nlhs, mxArray *plhs[])
{
  bool success = true;

  unsigned char*  pMatlabDataPri(NULL);
  unsigned char*  pMatlabDataSec(NULL);

  void*   pSharedData(NULL);
  size_t  nSharedData(0);

  if (success)
  {
    if (NULL==s_ptrMatrixPri)
    {
      mexPrintf("Failed to get matlab OpenGL array.\n");
      success = false;
    }
  }

  if (success)
  {
    if (NULL==s_ptrMatrixSec)
    {
      mexPrintf("Failed to get matlab Return array.\n");
      success = false;
    }
  }

  if (success)
  {
    pMatlabDataPri = reinterpret_cast<unsigned char*>(mxGetData(s_ptrMatrixPri));
    if (NULL==pMatlabDataPri)
    {
      mexPrintf("Failed to access matlab OpenGL array.\n");
      success = false;
    }
  }

  if (success)
  {
    pMatlabDataSec = reinterpret_cast<unsigned char*>(mxGetData(s_ptrMatrixSec));
    if (NULL==pMatlabDataSec)
    {
      mexPrintf("Failed to access matlab Return array.\n");
      success = false;
    }
  }

  if (success)
  {
    if (NULL==s_region)
    {
      mexPrintf("Failed to access shared memory array.\n");
      success = false;
    }
  }

  if (success)
  {
    pSharedData = s_region->get_address();
    if (NULL==pSharedData)
    {
      mexPrintf("Failed to access shared memory array.\n");
      success = false;
    }
  }

  if (success)
  {
    nSharedData = s_region->get_size();
    if (0U==nSharedData)
    {
      mexPrintf("Shared memory size is zero.\n");
      success = false;
    }
  }

  if (success)
  {
    if (NUM_OUTPUT_PARAMS!=nlhs)
    {
      mexPrintf("Wrong number of output parameters. (Got %i Expected %i).\n", nlhs, NUM_OUTPUT_PARAMS);
      success = false;
    }
  }

  if (success)
  {
    if (NULL==plhs)
    {
      mexPrintf("Invalid MEX pointer 'plhs'.\n");
      success = false;
    }
  }

  if (success)
  {
    try
    {
      // TODO: give up if locked out by XPlane   boost::posix_time::ptime(???);
      // TODO: give up if locked out by XPlane   if (s_mutex->timed_lock())
      {
        s_mutex->lock();
        memcpy(pMatlabDataPri, pSharedData, nSharedData);
        s_mutex->unlock();
      }
    }
    catch (interprocess_exception& err)
    {
      mexPrintf("Locking/Unlocking error.\n");
      mexPrintf("REASON: %s\n", err.what());
      success = false;
    }
    catch (...)
    {
      mexPrintf("Locking/Unlocking error.\n");
      mexPrintf("REASON: %s\n", "UNKNOWN EXCEPTION!");
      success = false;
    }
  }

  // Convert OpenGL frame byte order to expected matlab image format
  if (success)
  {
    // OpenGL RGB RGB RGB RGB 
    // Matlab RRRR GGGG BBBB  3 x [SCREENSHOT_WIDTH*SCREENSHOT_HEIGHT]
    // Plus the matlab memory is column major... :(
    // ...  the openGL memory is row major.
    int sourceRow = 0;
    int sourceCol = 0;
    int pixelOffset = 0;

    int sourceR_n = 0;
    int sourceG_n = 0;
    int sourceB_n = 0;

    int destR_n   = 0U*SCREENSHOT_WIDTH*SCREENSHOT_HEIGHT;
    int destG_n   = 1U*SCREENSHOT_WIDTH*SCREENSHOT_HEIGHT;
    int destB_n   = 2U*SCREENSHOT_WIDTH*SCREENSHOT_HEIGHT;

    for (sourceCol = 0U; sourceCol < SCREENSHOT_WIDTH; sourceCol++)
    {
      for (sourceRow = SCREENSHOT_HEIGHT-1; sourceRow >= 0; sourceRow--)
      {
        // Other RGB indices
        pixelOffset = (sourceRow * SCREENSHOT_WIDTH) + sourceCol;
        sourceR_n = SCREENSHOT_NUM_COLOR_PLANES * pixelOffset;
        sourceG_n = sourceR_n + 1;
        sourceB_n = sourceR_n + 2;

        // Copy and split data into colour planes
        pMatlabDataSec[destR_n] = pMatlabDataPri[sourceR_n];
        pMatlabDataSec[destG_n] = pMatlabDataPri[sourceG_n];
        pMatlabDataSec[destB_n] = pMatlabDataPri[sourceB_n];

        // Move to next locations in each "2D array"
        ++destR_n;
        ++destG_n;
        ++destB_n;
      }
    }
  }

  if (success)
  {
    // Give mxArray pointer back to matlab
    plhs[0] = s_ptrMatrixSec;
  }

  return success;
}

/*------------------------------------------------------------------------*/
